#include "simulation_model/simulation_model.h"

void SimulationMode::Init()
{
    v_ = 0;
    w_ = 0;
    charge_enable_ = false;
    is_charge_ = false;
    actuator_.is_auto_mode = 1;
    localization_.valid_flag = true;

    sub_control_   = nh_.subscribe("/control", 10, &SimulationMode::ControlCallback, this);
    sub_init_pose_ = nh_.subscribe("/initialpose", 10, &SimulationMode::InitPoseCallback, this);

    pub_pose_ = nh_.advertise<geometry_msgs::Pose2D>("/point_model_pose", 10);
    pub_local_ = nh_.advertise<localization_msgs::localization_msg>("/localization", 10);
    pub_actuator_ = nh_.advertise<actuator_msgs::actuator_msg>("/actuator", 10);
    float init_x, init_y, init_theta;
    nh_.param("x",     init_x,     0.0f);
    nh_.param("y",     init_y,     0.0f);
    nh_.param("theta", init_theta, 0.0f);
    model_base_ = std::make_shared<PointModel>(init_x, init_y, init_theta);
}

void SimulationMode::InitPoseCallback(const geometry_msgs::PoseWithCovarianceStamped &msg)
{
    float heading = atan2(2 * (msg.pose.pose.orientation.x * msg.pose.pose.orientation.y - 
                               msg.pose.pose.orientation.w * msg.pose.pose.orientation.z),
                              (msg.pose.pose.orientation.w * msg.pose.pose.orientation.w + 
                               msg.pose.pose.orientation.x * msg.pose.pose.orientation.x - 
                               msg.pose.pose.orientation.y * msg.pose.pose.orientation.y - 
                               msg.pose.pose.orientation.z * msg.pose.pose.orientation.z));
    
    model_base_->SetPose(msg.pose.pose.position.x, msg.pose.pose.position.y, -heading);
}

void SimulationMode::ControlCallback(const control_msgs::control_msg &msg)
{
    v_ = msg.speed;
    w_ = msg.angle_speed;
    v_toward_ = msg.speed_toward;
}

void SimulationMode::Start()
{
    ros::Rate rate(20);
    while (ros::ok())
    {
        model_base_->MoveModelProcess(v_, w_, v_toward_);
        localization_.pose = model_base_->GetPose();
        
        model_base_->GeSteerInfo(actuator_.walking_speed, actuator_.angle_speed, actuator_.speed_toward);
        actuator_.header.stamp = ros::Time::now();
        localization_.header.stamp = ros::Time::now();
        localization_.valid_flag = true;

        pub_local_.publish(localization_);
        pub_actuator_.publish(actuator_);

        rate.sleep();
        ros::spinOnce();
    }
}